#include "Update.h"
#include <FS.h>
#include <SPIFFS.h>
#include <ESPmDNS.h>
#include <WiFi.h>
#include <AsyncTCP.h>
#include <ESP8266FtpServer.h>
#include <ESPAsyncWebServer.h>
#include <esp_wifi.h>
#include "MAX30105.h"
#include "Wire.h"
#include <MPU6050_light.h>
#include "heartRate.h"
#include "WiFiUdp.h"
#include <freertos/FreeRTOS.h>
#include <vector>
#include <MPU6050_light.h>
#include "MPU6050_momo.h"



#pragma region ��������
FtpServer ftpSrv;   //set #define FTP_DEBUG in ESP8266FtpServer.h to see ftp verbose on serial
// SKETCH BEGIN
AsyncWebServer server(80);
const char* hostName = "hand_sensor";
String compile_time;
WiFiUDP udp;
IPAddress dest;
int32_t   dest_port;
String    dest_ip = "";
//-----------------------------
MAX30105 particleSensor;
//-------------------------------------max30102
const byte RATE_SIZE = 4; //Increase this for more averaging. 4 is good.
byte rates[RATE_SIZE]; //Array of heart rates
byte rateSpot = 0;
uint8_t rfid_buf[128] = { 0 };
uint8_t rfid_len = 0;
long lastBeat = 0; //Time at which the last beat occurred
float beatsPerMinute;
int beatAvg;
long irValue;
bool max30102_init_flag = false;
//--

uint8_t tx_buf1[64];
uint8_t rx_buf1[64];
uint8_t tx_buf2[64];
uint8_t rx_buf2[64];
uint8_t tx_buf3[64];
uint8_t rx_buf3[64];
SoftWire Wire4(25, 33);
SoftWire Wire2(4, 16);
SoftWire Wire3(27, 26);

MPU6050 mpu1(Wire1);
MPU6050V2 mpu2(Wire2);
MPU6050V2 mpu3(Wire3);
MPU6050V2 mpu4(Wire4);

int adc_counts = 0;
double out = 0;
float votage = 0;
uint8_t bat_level = 5;
float v = 0;
float bat_val[20] = { 0 };
#pragma endregion

bool bootMode = false;
uint64_t chipid = 0; 
void webserver_setup()
{
    server.serveStatic("/", SPIFFS, "/").setDefaultFile("main.html");
    server.onNotFound([](AsyncWebServerRequest* request) {
        Serial.printf("NOT_FOUND: ");
        if (request->method() == HTTP_GET)
            Serial.printf("GET");
        else if (request->method() == HTTP_POST)
            Serial.printf("POST");
        else if (request->method() == HTTP_DELETE)
            Serial.printf("DELETE");
        else if (request->method() == HTTP_PUT)
            Serial.printf("PUT");
        else if (request->method() == HTTP_PATCH)
            Serial.printf("PATCH");
        else if (request->method() == HTTP_HEAD)
            Serial.printf("HEAD");
        else if (request->method() == HTTP_OPTIONS)
            Serial.printf("OPTIONS");
        else
            Serial.printf("UNKNOWN");
        Serial.printf(" http://%s%s\n", request->host().c_str(), request->url().c_str());

        if (request->contentLength()) {
            Serial.printf("_CONTENT_TYPE: %s\n", request->contentType().c_str());
            Serial.printf("_CONTENT_LENGTH: %u\n", request->contentLength());
        }

        int headers = request->headers();
        int i;
        for (i = 0; i < headers; i++) {
            AsyncWebHeader* h = request->getHeader(i);
            Serial.printf("_HEADER[%s]: %s\n", h->name().c_str(), h->value().c_str());
        }

        int params = request->params();
        for (i = 0; i < params; i++) {
            AsyncWebParameter* p = request->getParam(i);
            if (p->isFile()) {
                Serial.printf("_FILE[%s]: %s, size: %u\n", p->name().c_str(), p->value().c_str(), p->size());
            }
            else if (p->isPost()) {
                Serial.printf("_POST[%s]: %s\n", p->name().c_str(), p->value().c_str());
            }
            else {
                Serial.printf("_GET[%s]: %s\n", p->name().c_str(), p->value().c_str());
            }
        }

        request->send(404);
        });
    server.onFileUpload([](AsyncWebServerRequest* request, const String& filename, size_t index, uint8_t* data, size_t len, bool final) {
        if (!index)
        {
            Serial.printf("UploadStart: %s\n", filename.c_str());
            if (filename.indexOf(".bin"))
            {
                if (Update.begin())
                {
                    Serial.println("begin update");
                }
            }
        }
        if (Update.write(data, len) != len)
        {
            Serial.println("write size error\n");
        }
        if (final)
        {
            Serial.printf("UploadEnd: %s (%u)\n", filename.c_str(), index + len);
            if (Update.hasError())
            {
                Serial.println("Error");
                Serial.println(Update.getError());
            }
            else
            {
                Update.end(true);
                request->send(200, "test/plain", "success");
                delay(1000);
                ESP.restart();
            }
        }
        });
    server.on("/set", [](AsyncWebServerRequest* request)
        {
            String ssid = "";
            String pass = "";
            String port = "";
            String dest_ip = "";
            for (int i = 0; i < request->args(); i++)
                Serial.println(request->arg(i).c_str());
            if (request->hasArg("AP_ssid"))
            {
                ssid = request->arg("AP_ssid");
                Serial.println(ssid);
            }
            if (request->hasArg("AP_pwd"))
            {
                pass = request->arg("AP_pwd");
                Serial.println(pass);

            }
            if (request->hasArg("Server_port"))
            {
                port = request->arg("Server_port");
                Serial.println(port);

            }
            if (request->hasArg("Server_IP"))
            {
                dest_ip = request->arg("Server_IP");
                Serial.println(dest_ip);
            }
            File f = SPIFFS.open("/setup.ini", "w+");
            if (f)
            {
                Serial.println("Write args.");
                f.println(ssid);
                f.println(pass);
                f.println(port);
                f.println(dest_ip);
                f.flush();
                f.close();
                request->send(200, "text/html", "<html><body><h1>Success,Please reset cpu...</h1></body></html>");
                delay(200);
                ESP.restart();
            }
            else
            {
                request->send(200, "text/html", "<html><body><h1>set fail</h1></body></html>");
            }
        });
    server.on("/ask", [](AsyncWebServerRequest* request)
        {
            String ssid = "";
            String pass = "";
            String port = "";
            String dest_ip = "";
            File f = SPIFFS.open("/setup.ini", "r");
            if (f)
            {
                ssid = f.readStringUntil('\n');
                pass = f.readStringUntil('\n');
                port = f.readStringUntil('\n');
                dest_ip = f.readStringUntil('\n');
                f.close();
            }
            request->send(200, "text/plain", "ssid=" + ssid + "\n"\
                "password=" + pass + "\n" + "dest_udp_port=" + port + "\n" + "dest_udp_ip=" + dest_ip);
        });
    server.begin();
}
void setup() {
    Serial.begin(9600);
    SPIFFS.begin(true);
    compile_time = __DATE__ + String(" ") + String(__TIME__);
    Serial.println();
    Serial.print("Compile time:");
    Serial.println(compile_time);
    delay(100);
    Serial.end();
    pinMode(3, INPUT_PULLUP);//ʹ��rx�� �ӵ� ���¸�λ����bootģʽ
    int32_t tick = 0;
    while (digitalRead(3) == LOW)
    {
        tick++;
        vTaskDelay(100);
        if (tick >= 10)
        {
            bootMode = true;
            break;
        }
    }
    Serial.begin(9600);
    chipid=ESP.getEfuseMac();
    Serial.printf("%X\n",chipid);
    if (bootMode)
    {
        Serial.println("enter boot mode");
        SPIFFS.remove("/setup.ini");
        esp_wifi_restore();
        WiFi.softAP("hand", "12345678");
        ftpSrv.begin("ftp", "ftp");
    }
    else
    {
        Serial.println("Enter normal mode");
        //�ļ������� Ĭ�ϴ�apģʽ
        if (!SPIFFS.exists("/setup.ini"))
        {
            esp_wifi_restore();
            WiFi.softAP("hand", "12345678");
        }
        else
        {
            File file = SPIFFS.open("/setup.ini", "r");
            if (file)
            {
                String ssid = file.readStringUntil('\n');
                String pass = file.readStringUntil('\n');
                String port = file.readStringUntil('\n');
                String ip = file.readStringUntil('\n');
                file.close();
                //delete '\n'
                ssid = ssid.substring(0, ssid.length() - 1);
                pass = pass.substring(0, pass.length() - 1);
                port = port.substring(0, port.length() - 1);
                ip = ip.substring(0, ip.length() - 1);
                Serial.println("ssid:" + ssid);
                Serial.println("password:" + pass);
                Serial.println("udp dest port:" + port);
                Serial.println("udp dest ip:" + ip);
                WiFi.mode(WIFI_STA);
                WiFi.begin(ssid.c_str(), pass.c_str());
                int counts = 0;
                while (WiFi.status() != WL_CONNECTED)
                {
                    delay(500);
                    if (counts++ >= 20)
                    {
                        Serial.printf("Wifi connect failed");
                        SPIFFS.remove("/setup.ini");
                        WiFi.softAP("hand", "12345678");
                        break;
                    }
                }
                if (WiFi.status() == WL_CONNECTED)
                {
                    dest.fromString(ip);
                    dest_port = atoi(port.c_str());
                    dest_ip = ip;
                }
            }
        }
        //����httpserver
        Wire1.begin(12,14);//��һ�鲻��������I2C��  Ӳ��I2C
        Wire.begin(23, 22);//->����Ѫ��I2C         Ӳ��I2C
        Wire2.begin();//->���I2C
        Wire3.begin();//->
        Wire4.begin();//->

        Wire2.setTimeout_ms(40);
        Wire2.setClock(400000);
        Wire2.setTxBuffer(tx_buf1, 64);
        Wire2.setRxBuffer(rx_buf1, 64);
        Wire2.begin();

        Wire3.setTimeout_ms(40);
        Wire3.setClock(400000);
        Wire3.setTxBuffer(tx_buf2, 64);
        Wire3.setRxBuffer(rx_buf2, 64);
        Wire3.begin();

        Wire4.setTimeout_ms(40);
        Wire4.setClock(400000);
        Wire4.setTxBuffer(tx_buf3, 64);
        Wire4.setRxBuffer(rx_buf3, 64);
        Wire4.begin();

        adcAttachPin(32);
        adcStart(32);
        xTaskCreatePinnedToCore(max30102_loop, "max30102", 4096, NULL, 8, NULL, 0);
        xTaskCreatePinnedToCore(mpu_loop_cycle, "mpu", 4096, NULL, 5, NULL, 1);
        xTaskCreatePinnedToCore(bat_rfid_loop, "bat_rf", 4096, NULL, 6, NULL, 0);

    }
    xTaskCreatePinnedToCore(ftp, "ftp", 4096, NULL, 9, NULL, 1);
}
void ftp(void*)
{
    //����ftp����
    //Ĭ���˺����� ���� ftp
    ftpSrv.begin("ftp", "ftp");    //username, password for ftp.  set ports in ESP8266FtpServer.h  (default 21, 50009 for PASV)
    MDNS.addService("http", "tcp", 80);
    webserver_setup();
    while (1)
    {
        vTaskDelay(1);
        ftpSrv.handleFTP();
    }
}
void mpu_loop_cycle(void*)
{
    uint8_t error = 0;
    vTaskDelay(10);
    //���߼��
    Wire1.beginTransmission(0x68);
    error = Wire1.endTransmission();
    if (error == 0)
    {
        Serial.println("mpu1 init success");
    }
    else
    {
        Serial.println("mpu1 init failed");
    }
    vTaskDelay(10);
    Wire2.beginTransmission(0x68);
    error = Wire2.endTransmission(true);
    if (error == 0)
    {
        Serial.println("mpu2 init success");
    }
    else
    {
        Serial.println("mpu2 init failed");
    }
    vTaskDelay(10);
    Wire3.beginTransmission(0x68);
    error = Wire3.endTransmission(true);
    if (error == 0)
    {
        Serial.println("mpu3 init success");
    }
    else
    {
        Serial.println("mpu3 init failed");
    }
    vTaskDelay(10);
    Wire4.beginTransmission(0x68);
    error = Wire4.endTransmission(true);
    if (error == 0)
    {
        Serial.println("mpu4 init success");
    }
    else
    {
        Serial.println("mpu4 init failed");
    }
    Serial.println("calibration,Waiting And not move. 5s");
    mpu1.begin();
    vTaskDelay(1000);
    mpu1.calcGyroOffsets();

    mpu2.begin();
    vTaskDelay(1000);
    mpu2.calcGyroOffsets();
    mpu3.begin();
    vTaskDelay(1000);
    mpu3.calcGyroOffsets();

    mpu4.begin();
    vTaskDelay(1000);
    mpu4.calcGyroOffsets();
    Serial.println("calibration finish.");
    int ticks = 0;
    while (1)
    {
        if (WiFi.status() == WL_CONNECTED && WiFi.getMode() == WIFI_STA && dest_ip != "" && dest_port != 0)
        {
            if (ticks++ % 8 == 0)
            {
                udp.beginPacket(dest, dest_port);
                udp.print("$HANDMPU,");
                udp.print(mpu1.getAccAngleX());
                udp.print(",");
                udp.print(mpu1.getAccAngleY());
                udp.print(",");
                udp.print(mpu2.getAccAngleX());
                udp.print(",");
                udp.print(mpu2.getAccAngleY());
                udp.print(",");
                udp.print(mpu3.getAccAngleX());
                udp.print(",");
                udp.print(mpu3.getAccAngleY());
                udp.print(",");
                udp.print(mpu4.getAccAngleX());
                udp.print(",");
                udp.print(mpu4.getAccAngleY());
                udp.print(",");
                udp.print("*FF");
                udp.println();
                udp.endPacket();
            }
        }
        mpu1.update();
        mpu2.update();
        mpu3.update();
        mpu4.update();
        vTaskDelay(1);
    }
}
void max30102_loop(void*)
{
    if (!particleSensor.begin(Wire, I2C_SPEED_FAST)) //Use default I2C port, 400kHz speed
    {
        Serial.println("MAX30105 was not found. Please check wiring/power. ");
    }
    else
    {
        max30102_init_flag = true;
        Serial.println("Place your index finger on the sensor with steady pressure.");

        particleSensor.setup(); //Configure sensor with default settings
        particleSensor.setPulseAmplitudeRed(0x0A); //Turn Red LED to low to indicate sensor is running
        particleSensor.setPulseAmplitudeGreen(0); //Turn off Green LED
    }
    while (1)
    {
        if (WiFi.status() == WL_CONNECTED && WiFi.getMode() == WIFI_STA && dest_ip != "" && dest_port != 0)
        {
            if (max30102_init_flag)
            {
                irValue = particleSensor.getIR();
                if (checkForBeat(irValue) == true)
                {
                    //We sensed a beat!
                    long delta = millis() - lastBeat;
                    lastBeat = millis();
                    if (delta == 0) delta = 1;
                    beatsPerMinute = 60 / (delta / 1000.0);

                    if (beatsPerMinute < 255 && beatsPerMinute > 20)
                    {
                        rates[rateSpot++] = (byte)beatsPerMinute; //Store this reading in the array
                        rateSpot %= RATE_SIZE; //Wrap variable

                        //Take average of readings
                        beatAvg = 0;
                        for (byte x = 0; x < RATE_SIZE; x++)
                            beatAvg += rates[x];
                        beatAvg /= RATE_SIZE;
                    }
                }
            }
        }
        vTaskDelay(1);
    }
}
void bat_rfid_loop(void*)
{
    int ticks = 0;
    while (1)
    {
        if (WiFi.status() == WL_CONNECTED && WiFi.getMode() == WIFI_STA && dest_ip != "" && dest_port != 0)
        {
            bat_loop();
            rfid_loop();
            if (ticks++ % 100 == 0)
            {
                if (irValue <= 3000)
                {
                    beatsPerMinute = 0;
                    beatAvg = 0;
                }
                udp.beginPacket(dest, dest_port);
                udp.print("$HANDBAT,");
                udp.print(votage);
                udp.print(",");
                udp.print(bat_level);
                udp.print("*FF");
                udp.println();
                udp.endPacket();
                udp.beginPacket(dest, dest_port);
                udp.print("$HANDBEAT,");
                udp.print(beatsPerMinute);
                udp.print(",");
                udp.print(beatAvg);
                udp.print(",");
                udp.print(irValue);
                udp.print("*FF");
                udp.println();
                udp.endPacket();
                udp.beginPacket(dest, dest_port);
                udp.printf("$CHIPID,%X,*FF\n",chipid);
                udp.endPacket();
            }
        }
        vTaskDelay(5);
    }
}

void bat_loop()
{
    float v = analogRead(32) / 4096.0 * 3.3 * 2;
    v += 0.4;
    if (adc_counts < 20)
    {
        bat_val[adc_counts] = v;
        adc_counts++;
    }
    else
    {
        for (int i = 0; i < 20 - 1; i++)
        {
            for (int j = 0; j < 20 - i - 1; j++)
            {
                if (bat_val[j] > bat_val[j + 1]) {
                    float temp = bat_val[j];
                    bat_val[j] = bat_val[j + 1];
                    bat_val[j + 1] = temp;
                }
            }
        }
        out = 0;
        for (int j = 12; j < 16; j++)
        {
            out += bat_val[j];
        }
        votage = out / 4.0;
        adc_counts = 0;
    }
    if (votage >= 3.8)
        bat_level = 5;
    else if (votage < 3.8 && votage >= 3.6)
        bat_level = 4;
    else if (votage < 3.6 && votage >= 3.2)
        bat_level = 3;
    else if (votage < 3.2 && votage >= 2.8)
        bat_level = 2;
    else if (votage < 2.8)
        bat_level = 1;

}
void rfid_loop()
{
    int len = Serial.available();
    if (len > 0 && len <= 128)
    {
        rfid_len = Serial.readBytes(rfid_buf, len);
        Serial.printf("rfid get %d", rfid_len);
        udp.beginPacket(dest, dest_port);
        udp.print("$HANDRF,");
        for (int i = 0; i < rfid_len; i++)
            udp.print(rfid_buf[i], HEX);
        udp.print("*FF");
        udp.println();
        udp.endPacket();
    }
}

void loop()
{
}
